#pragma once

namespace grflib
{
	namespace LA
	{
		//Zero indexed, fixed size vector
		template<typename T>
		class Vec
		{
		public:
			Vec(void);
			Vec(UINT length);
			Vec(const Vec &v);
			virtual ~Vec(void);
			Vec & operator=(const Vec &v);

			bool SetSize(UINT length);
			void Release(void);

			T & operator() (UINT i);
			const T & operator() (UINT i) const;

			inline T * GetBuffer(void) {return m_data;}
			inline UINT GetSize(void) const {return m_length;}

			void SetValue(const T& value);

			T Dot(const Vec<T> &rhs);
			T Dot(const Vec<T> &rhs1, const Vec<T> &rhs2);
			T Length(const Vec<T> &rhs);
			T LengthSq(const Vec<T> &rhs);
			void QuickNormalize(const Vec<T> & rhs);
			void Normalize(const Vec<T> & rhs);


			// From Quake III. But the magic number is from http://www.lomont.org/Math/Papers/2003/InvSqrt.pdf
			inline float
				RecipSqrt(float number)
			{
				float const threehalfs = 1.5f;

				float x2 = number * 0.5f;
				union FNI
				{
					float f;
					int i;
				} fni;
				fni.f = number;											// evil floating point bit level hacking
				fni.i = 0x5f375a86 - (fni.i >> 1);						// what the fuck?
				fni.f = fni.f * (threehalfs - (x2 * fni.f * fni.f));	// 1st iteration
				//fni.f = f * (threehalfs - (x2 * fni.f * fni.f));		// 2nd iteration, this can be removed

				return fni.f;
			}

		protected:
			T *m_data;
			UINT m_length;
		};

		template<typename T>
		Vec<T>::Vec() :
			m_data(0),
			m_length(0)
		{
		}

		template<typename T>
		Vec<T>::Vec(UINT length)
		{
			m_data = new T[length];
			if (m_data)
				m_length = length;
		}

		template<typename T>
		Vec<T>::Vec(const Vec &v)
		{
			m_data = new T[v.m_length];
			if (m_data)
			{
				m_length = v.m_length;
				memcpy(m_data, v.m_data, sizeof(T) * m_length);
			}
		}

		template<typename T>
		Vec<T> & Vec<T>::operator= (const Vec<T> &v)
		{
			if (this == &v)
				return (*this);

			SetSize(v.m_length);
			memcpy(m_data, v.m_data, sizeof(T) * m_length);
			return (*this);
		}

		template<typename T>
		bool Vec<T>::SetSize(UINT length)
		{
			if (m_length != length)
			{
				Release();
				if (0 == (m_data = new T[length]))
					return false;
				m_length = length;
			}
			return true;
		}

		template<typename T>
		Vec<T>::~Vec()
		{
			Release();
		}

		template<typename T>
		void Vec<T>::Release()
		{
			if (m_data)
			{
				delete []m_data;
				m_data = 0;
			}
			m_length = 0;
		}

		template<typename T>
		T & Vec<T>::operator() (UINT i)
		{
			return *(m_data + i);
		}
		
		template<typename T>
		const T & Vec<T>::operator() (UINT i) const
		{
			return *(m_data + i);
		}

		template<typename T>
		T Vec<T>::Dot(const Vec<T> &rhs)
		{
			T temp = 0.0f;
			for ( size_t i = 0; i < m_length; ++i )
			{
				temp += m_data[i] * rhs(i);
			}
			return temp;
		}

		template<typename T>
		T Vec<T>::Dot(const Vec<T> &rhs1, const Vec<T> &rhs2)
		{
			T temp = 0.0f;
			for ( size_t i = 0; i < m_length; ++i )
			{
				temp += rhs1(i) * rhs2(i);
			}
			return temp;
		}

		template<typename T>
		T Vec<T>::Length(const Vec<T> & rhs)
		{
			return std::sqrt(LengthSq(rhs));
		}

		template<typename T>
		T Vec<T>::LengthSq(const Vec<T> & rhs)
		{
			return Dot(rhs, rhs);
		}

		template<typename T>
		void Vec<T>::QuickNormalize(const Vec<T> & rhs)
		{
			const T inv = RecipSqrt(LengthSq(rhs));
			for (UINT i = 0; i < m_length; ++i)
			{
				*(m_data + i) *= inv;
			}
		}

		template<typename T>
		void Vec<T>::Normalize(const Vec<T> & rhs)
		{
			const T inv = 1.0f/Length(rhs);
			for (UINT i = 0; i < m_length; ++i)
			{
				*(m_data + i) *= inv;
			}
		}
		

		template<typename T>
		void Vec<T>::SetValue(const T &value)
		{
			for (UINT i = 0; i < m_length; i++)
			{
				*(m_data + i) = value;
			}
		}
	} //namespace namespace LA
} //namespace namespace grflib